package it.polito.connectedcar.vehicle;

import it.polito.connectedcar.ConnectedObject;
import it.polito.connectedcar.message.GoodbyeMessage;
import it.polito.connectedcar.message.MessageQueueFactory;
import it.polito.connectedcar.message.TCCMessageListener;
import it.polito.connectedcar.message.TCCMessageProcessor;
import it.polito.connectedcar.message.VehicleFromTCCMessage;
import it.polito.connectedcar.message.VehicleFromVehicleMessage;
import it.polito.connectedcar.message.VehicleMessageListener;
import it.polito.connectedcar.message.VehicleMessageProcessor;
import it.polito.connectedcar.message.VehicleToVehicleMessage;
import it.polito.connectedcar.tcc.TCCTrafficLightPhaseChange;
import it.polito.connectedcar.tcc.TCCTrafficLightPhaseChange.TRAFFICLIGHT_STATE;
import it.polito.connectedcar.tcc.zone.Token;
import it.polito.connectedcar.util.ConnectedCarUtil;
import it.polito.connectedcar.util.Constants;
import it.polito.connectedcar.util.TestZones;
import it.polito.connectedcar.vehicle.network.VehicleNetworkConnectionManager;

import java.awt.geom.Point2D;
import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStream;
import java.io.InputStreamReader;
import java.io.OutputStream;
import java.net.InetSocketAddress;
import java.net.MalformedURLException;
import java.net.SocketException;
import java.net.URL;
import java.net.URLConnection;
import java.util.List;
import java.util.Map.Entry;
import java.util.Set;
import java.util.concurrent.CopyOnWriteArraySet;

/**
 * The highest representation of a vehicle inside the ConnectedCars system. It
 * deals with all the information coming from other entities though the network
 * and takes decisions upon them. It process information coming from :
 * <ul>
 * <li>
 * TCC -such as the list of vehicles ,traffic lights info, a new zone or a
 * change zone command:</li>
 * <li>Vehicle- vehicle's position,direction, speed and belonging token</li>
 * 
 * based on the cntinuously updating information , it can performs collision
 * warning , traffic light checks and consequently applying speed reduction
 * 
 * @author Nicola Aresta
 * 
 */
public class ConnectedVehicle extends ConnectedObject
		implements
			VehicleMessageListener,
			TCCMessageListener,
			VehicleUpdateDelegate {

	private static final double TIME_SLICE_COEFFICIENT = 4; // expressed in
															// seconds
	private static final double MAXIMUM_ALERT_TIME_DISTANCE = 10; // expressed
																	// in
																	// seconds:
																	// minimum
																	// time for
																	// collisions
																	// to
																	// consider
	private static final double MINIMUM_DISTANCE_THRESHOLD_TO_ALERT = 2;// expressed
																		// in m
	private static final double MINIMUM_SPEED_THRESHOLD_TO_ALERT = 10; // expressed
																		// in
																		// Km/h
	private static final long OLDEST_ACCEPTED_DATA_TIMESTAMP_THRESHOLD = 20; // expressed
																				// in
																				// seconds

	private VehicleMessageProcessor vmp;
	private TCCMessageProcessor tmp;
	private VehicleNeighborhood vn;
	private VehicleNetworkConnectionManager vncm;
	private SourceVehicleProxy sourceVehicle;
	private boolean connected = false;
	private TCCTrafficLightPhaseChange tccTrafficLightPhaseChange;
	private long lastUpdateTimestamp;

	public ConnectedVehicle(SourceVehicleProxy sourceVehicle)
			throws SocketException {
		super();

		this.sourceVehicle = sourceVehicle;
		sourceVehicle.setVehicleUpdateDelegate(this);

		vn = new VehicleNeighborhood();
		MessageQueueFactory messageQueueFactory = new MessageQueueFactory();
		vncm = new VehicleNetworkConnectionManager(vn, messageQueueFactory);
		vmp = new VehicleMessageProcessor(vn, messageQueueFactory);
		tmp = new TCCMessageProcessor(vn, messageQueueFactory);
		vmp.setVehicleMessageListener(this);
		tmp.setTccMessageListener(this);
		vmp.start();
		tmp.start();
	}

	/**
	 * 
	 * @param title
	 * @param message
	 */
	public void showAlert(String title, String message) {
		System.err.println("alert of "
				+ sourceVehicle.getVehicleIdentificationNumber() + " : "
				+ title + "   " + message);

	}

	/**
	 * 
	 * @param title
	 * @param message
	 */
	public void showInfo(String title, String message) {
		System.err.println("info : " + title + "   " + message);
	}

	@Override
	public void onMessageReceived(VehicleFromVehicleMessage message) {
		// System.out.println("VEHICLE : "+sourceVehicle.getVehicleIdentificationNumber()+" received message from "+message.getSourceAddress()+" using token "+message.getBelongingToken().toString());
		performCollisionsWarning(message);
	}

	/**
	 * The main logic to perform collision warning. Based on the vehicle's
	 * position, direction and speed , it finds out whether the 2 vehicles'
	 * trajectories can collide in the near future or not. If collisions might
	 * happen , it forecasts the time after which the collision may happen and
	 * if this value is under a certain threshold , it fires an alert to inform
	 * about the danger.
	 * 
	 * @param message
	 */
	private void performCollisionsWarning(VehicleFromVehicleMessage message) {

		/*
		 * positions are UTM because since they are near sue to interesting
		 * quadrants, the earth can be considered plain and the coordinates are
		 * projection
		 */
		double otherDirection = message.getDirection();
		double sourceDirection = sourceVehicle.getDirection();
		double sourceXPos = sourceVehicle.getPosition().getX();
		double sourceYPos = sourceVehicle.getPosition().getY();
		double sourceSpeed = Math.floor(sourceVehicle.getSpeed());// m/s
		double otherSpeed = Math.floor(message.getSpeed()); // m/s

		long lastUpdateTimeStampClone = lastUpdateTimestamp;

		// FIXME not sure about the result
		Double otherXPos = message.getUtmXPos();
		Double otherYPos = message.getUtmYPos();
		/*
		 * since the message can be subject to delays due to the network, we
		 * must consider the time when the measure have been taken
		 */
		double distanceBetweenVehicles = Math.floor(Point2D.distance(
				sourceXPos, sourceYPos, otherXPos, otherYPos));

		System.out.println("distace between vehicles "
				+ sourceVehicle.getVehicleIdentificationNumber() + " and "
				+ message.getSourceAddress() + " is " + distanceBetweenVehicles
				+ " m");
		Integer newPriority = calculatePriority(distanceBetweenVehicles, null,
				-1, -1);

		/*
		 * Based on the distance , the priority of the link connected to the
		 * vehicle sending the message changes
		 */
		VehicleZone belongingZone = vn.getNeighbors().get(
				vn.getNeighbors().indexOf(message.getBelongingToken()));

		/* check the newly calculated priority */
		CopyOnWriteArraySet<InetSocketAddress> newPriorityLists = belongingZone
				.getPrioritizedVehicles().get(newPriority);

		/* lazy initialization */
		// FIXME --> initialization when necessary

		if (newPriorityLists == null) {
			newPriorityLists = new CopyOnWriteArraySet<InetSocketAddress>();
			belongingZone.getPrioritizedVehicles().put(newPriority,
					newPriorityLists);
		}

		if (!newPriorityLists.contains(message.getSourceAddress())) {
			for (Integer priority : belongingZone.getPrioritizedVehicles()
					.keySet()) {
				if (priority == newPriority)
					continue;

				Set<InetSocketAddress> list = belongingZone
						.getPrioritizedVehicles().get(priority);
				if (list.contains(message.getSourceAddress())) {
					// System.out.println("VEHICLE "+sourceVehicle.getVehicleIdentificationNumber()+" changed priority of "+message.getSourceAddress().toString()+" from "+priority+" to "+newPriority);
					list.remove(message.getSourceAddress());
					break;
				}
			}
			newPriorityLists.add(message.getSourceAddress());
		}

		// System.out.println("vehicle "+sourceVehicle.getVehicleIdentificationNumber()+
		// "distant "+distanceBetweenVehicles+
		// "from "+message.getVehicleIdentifier());

		if (distanceBetweenVehicles < MINIMUM_DISTANCE_THRESHOLD_TO_ALERT
				&& (sourceSpeed > MINIMUM_SPEED_THRESHOLD_TO_ALERT || otherSpeed > MINIMUM_SPEED_THRESHOLD_TO_ALERT)) {
			// alert collisions
			showAlert("collision warning",
					"YOU " + sourceVehicle.getVehicleIdentificationNumber()
							+ " (" + sourceSpeed + " km/h ) ARE TOO NEAR WITH "
							+ message.getVehicleIdentifier() + " ("
							+ otherSpeed + " km/h) -->distance "
							+ distanceBetweenVehicles + " m");
		}

		/* consider the timestamp when the message has been sent */

		long messageDataTime = message.getTimestamp();
		long now = System.currentTimeMillis();
		if ((now - messageDataTime) / 1000 > OLDEST_ACCEPTED_DATA_TIMESTAMP_THRESHOLD) {
			// this means that the data is too old to be considered
			return;
		}

		if (message.getCurrentLaneId().equals(sourceVehicle.getCurrentLaneId())) {
			// they are on the same laneId
		}

		/*
		 * Consider the vertical direction case
		 */
		double sourceDirection_rad = Math.toRadians(sourceDirection);
		double otherDirection_rad = Math.toRadians(otherDirection);

		Point2D intersection = ConnectedCarUtil.raysIntersection(sourceXPos,
				sourceYPos, sourceXPos + Math.cos(sourceDirection_rad),
				sourceYPos + Math.sin(sourceDirection_rad), otherXPos,
				otherYPos, otherXPos + Math.cos(otherDirection_rad), otherYPos
						+ Math.sin(otherDirection_rad));

		boolean isCollision = false;
		long collisionTime = -1;
		if (intersection == null) {
			// they are parallels... check the same line case
			/*
			 * solve the equation of a line passing through 2 points-> if the
			 * angle is the same , they lie on the same line
			 */
			boolean lieOnSameLine = (otherYPos - sourceYPos == sourceDirection
					* (otherYPos - sourceYPos));
			if (lieOnSameLine) {
				// they lie on the same line
				double distanceFromOriginToOther = Point2D.distance(0, 0,
						otherXPos, otherYPos);
				double distanceFromOriginToSource = Point2D.distance(0, 0,
						sourceXPos, sourceYPos);
				if (sourceDirection == otherDirection) {
					// they can collide because they have the same direction

					if (distanceFromOriginToSource > distanceFromOriginToOther
							&& otherSpeed > sourceSpeed) {
						// the source vehicles is in front of the source vehicle
						// , calculate how much time the vehicle can reach the
						// back vehicle
						collisionTime = (long) (distanceBetweenVehicles / (otherSpeed - sourceSpeed));
						isCollision = true;

					} else if (distanceFromOriginToOther > distanceFromOriginToSource
							&& sourceSpeed > otherSpeed) {
						// the other vehicle is in front of the other vehicle
						collisionTime = (long) (distanceBetweenVehicles / (sourceSpeed - otherSpeed));
						isCollision = true;
					}

				} else if (distanceFromOriginToSource > distanceFromOriginToOther
						&& sourceDirection >= 180
						|| distanceFromOriginToOther > distanceFromOriginToSource
						&& otherDirection >= 180) {
					/*
					 * the vehicles have opposite direction but the interesting
					 * case : they're directions converge
					 */
					collisionTime = (long) (((distanceBetweenVehicles - sourceSpeed) / (sourceSpeed + otherSpeed)) / sourceSpeed);// in
																																	// seconds
					isCollision = true;
				}
			} else
				/*
				 * the 2 vehicles are simply parallel but they don't lie on the
				 * same line
				 */
				return;
		} else {
			/* distances in meters */
			double distanceTo1 = Point2D.distance(sourceXPos, sourceXPos,
					intersection.getX(), intersection.getY());
			double distanceTo2 = Point2D.distance(otherXPos, otherYPos,
					intersection.getX(), intersection.getY());

			/*
			 * calculate absolute starting from the measure time ( in
			 * milliseconds)
			 */
			long t1 = lastUpdateTimeStampClone
					+ (long) ((sourceSpeed / distanceTo1) * 1000);// t1 is when
																	// the
																	// source
																	// vehicle
																	// is
																	// expected
																	// to reach
																	// the
																	// intersection
																	// point
																	// between
																	// the 2
																	// routes
			long t2 = messageDataTime
					+ (long) ((otherSpeed / distanceTo2) * 1000);// t2 is when
																	// the other
																	// vehicle
																	// is
																	// expected
																	// to reach
																	// the
																	// intersection
																	// point
																	// between
																	// the 2
																	// routes

			/*
			 * The collision point is either before in time with respect to now
			 * , or too far in time. So we don't need to place an alert because
			 * either one of the vehicles changes its direction , or they will
			 * receive more useful updates later on.
			 */
			if (t1 < now || t2 < now
					|| ((t1 - now) / 1000 > MAXIMUM_ALERT_TIME_DISTANCE)
					|| ((t2 - now) / 1000 > MAXIMUM_ALERT_TIME_DISTANCE)) {
				return;
			}

			/*
			 * If they reach the same point approximatively in the same time but
			 * a small error constant, an alert is placed to the driver
			 */
			collisionTime = Math.abs(t1 - t2);
			if (collisionTime < TIME_SLICE_COEFFICIENT)
				isCollision = true;
		}

		if (isCollision && collisionTime > 0)
			showAlert(
					"collision warning",
					"a vehicle ( " + message.getVehicleIdentifier()
							+ " ) is approaching you ("
							+ sourceVehicle.getVehicleIdentificationNumber()
							+ ") in :" + collisionTime
							+ " seconds.. pay attention");
	}

	/**
	 * Prioritized the links between vehicles is a way to spare communication
	 * bandwidth and computational time needed by receiving counterparts. By
	 * giving a priority to the current relative positions of 2 vehicles , we
	 * can decide how frequently a message update must be sent to such vehicle.
	 * The priorities are continuously changing based on the movement of
	 * vehicles in the space.
	 * 
	 * 
	 * @param distanceBetweenVehicles
	 *            the on air distance between vehicles (meters)
	 * @param iPoint
	 *            intersection point between the 2 vehicles' routes (UTM
	 *            coordinates)
	 * @param t1
	 *            time needed by the current vehicle to reach the intersection
	 *            point if any (seconds)
	 * @param t2
	 *            time needed by the other vehicle to reach the intersection
	 *            point (seconds)
	 * @return the priority index based on the above parameters.
	 */
	private Integer calculatePriority(double distanceBetweenVehicles,
			Point2D iPoint, double t1, double t2) {

		return Math.min((int) distanceBetweenVehicles
				/ VehicleZone.DISTANCE_FOR_PRIORITY_LEVEL,
				VehicleZone.MINIMUM_PRIORITY);
	}

	/**
	 * 
	 */
	@Override
	public void onMessageReceived(VehicleFromTCCMessage message) {
		/*
		 * inside this message there can be :
		 * 
		 * 1- a list of vehicles associated to a token 2- a new token to take
		 * into account( Token + TCC address ) 3- data coming from
		 * trafficLights( position of the traffic Light in UTM coordinates,
		 * current color, nextColorElapsedTime 4- data coming from inductive
		 * loops ( how busy is a road )
		 */

		if (message.getVehiclesToAdd() != null) {

			for (Entry<Token, List<InetSocketAddress>> entry : message
					.getVehiclesToAdd().entrySet()) {
				// System.out.println("VEHICLE "+sourceVehicle.getVehicleIdentificationNumber()+" received vehicles from token : "+entry.getKey());
				VehicleZone vz = vn.getNeighbors().get(
						vn.getNeighbors().indexOf(entry.getKey()));

				synchronized (vz) {
					// vz.getVehicles().clear();
					vz.getPrioritizedVehicles().clear();

					List<InetSocketAddress> value = entry.getValue();
					for (InetSocketAddress address : value) {
						if (!address.equals(vncm.getAddress())) {
							vz.getVehicles().add(address);

							// give to the vehicle the maximum priority
							CopyOnWriteArraySet<InetSocketAddress> list = vz
									.getPrioritizedVehicles().get(
											VehicleZone.MAXIMUM_PRIORITY);
							if (list == null) {
								list = new CopyOnWriteArraySet<InetSocketAddress>();
								vz.getPrioritizedVehicles().put(
										VehicleZone.MAXIMUM_PRIORITY, list);

							}
							list.add(address);
						}
					}
				}
			}

		} else if (message.getZonesToAdd() != null) {
			for (VehicleZone vz : message.getZonesToAdd()) {
				if (!vn.getNeighbors().contains(vz)) {
					// System.out.println("VEHICLE : "+sourceVehicle.getVehicleIdentificationNumber()+" received new zone : "+vz.getZoneToken().toString());
					vn.getNeighbors().add(vz);
				}

			}

		} else if (message.getChangedToken() != null) {
			/*
			 * send to last Token TCC a goodbye message set the received token
			 * as the actual token
			 */

			int indexOfNextCurrentZone = vn.getNeighbors().indexOf(
					message.getChangedToken());
			if (indexOfNextCurrentZone != -1) {
				VehicleZone nextCurrentZone = vn.getNeighbors().get(
						indexOfNextCurrentZone);
				VehicleZone currentZone = vn.getCurrentZone();
				if (currentZone != nextCurrentZone) {
					synchronized (currentZone) {
						vn.getNeighbors().remove(currentZone);
						// System.out.println("VEHICLE : "+sourceVehicle.getVehicleIdentificationNumber()+" remove zone "+currentZone.getZoneToken());
						GoodbyeMessage gb = new GoodbyeMessage(vn
								.getCurrentZone().getZoneToken());
						vn.setCurrentZone(nextCurrentZone);
						gb.setForTCC(true);
						gb.setForZones(false);
						gb.setVehicleIdentifier(sourceVehicle
								.getVehicleIdentificationNumber());
						gb.setTccAddress(vn.getCurrentZone().getTccAddress());
						try {
							vncm.sendMessage(gb);
							// System.out.println("VEHICLE : "+sourceVehicle.getVehicleIdentificationNumber()+" changed Token to "+nextCurrentZone.getZoneToken());
						} catch (IOException e) {
							e.printStackTrace();
						}
					}
				}

			} else {
				// drop the packet
				System.out.println("packet dropped by vehicle");
			}

		} else if (message.getTrafficLightsPhaseChange() != null) {
			/*
			 * read the traffic light's position, calculate the distance to
			 * reach the traffic light. if the current phase is RED , and given:
			 * -the instant when it will be GREEN -the current vehicle speed ->
			 * adjust the speed of the vehicle so that it can reach the traffic
			 * light when it becomes green, it can pass through and start
			 * accelerating again
			 */

			this.tccTrafficLightPhaseChange = message
					.getTrafficLightsPhaseChange();

		} else if (message.getInductiveLoopsInfo() != null) {
			// inductive loop strategy
		}
	}

	/**
	 * In order to avoid useless computations and takes wrong decisions , a
	 * Traffic Light has to be considered only when it is in front of the
	 * vehicles along its direction.
	 * 
	 * @param tccTrafficLightPhaseChange
	 * @return <b>true</>if the traffic light is behind the vehicle, false
	 *         otherwise
	 */
	private boolean isTrafficLightBehind(
			TCCTrafficLightPhaseChange tccTrafficLightPhaseChange) {

		boolean isBehind = false;
		double direction = sourceVehicle.getDirection();

		int tlDistanceMinimumGap = 10;
		double x = sourceVehicle.getPosition().getX();
		double y = sourceVehicle.getPosition().getY();
		double tlX = tccTrafficLightPhaseChange.getPosition().getX();
		double tlY = tccTrafficLightPhaseChange.getPosition().getY();
		double distance = Point2D.distance(x, y, tlX, tlY);
		double directedY = y + Math.sin(Math.toRadians(direction));
		double directedX = x + Math.cos(Math.toRadians(direction));
		double directedDistance = Point2D.distance(directedX, directedY, tlX,
				tlY);

		if (distance <= tlDistanceMinimumGap || distance <= directedDistance) {
			// System.out.println("the traffic light for the vehicle : "+sourceVehicle.getVehicleIdentificationNumber()+" is behind..... so it will be ignored");
			isBehind = true;
		}
		return isBehind;
	}

	@Override
	public void onSpeedChange(double speed) {
		/*
		 * actually we don't need to fire anything when this value changed
		 * because normally is immediately followed by a lcation change
		 */
	}

	@Override
	public void onDirectionChange(double angle) {
		/*
		 * actually we don't need to fire anything when this value changed
		 * because normally is immediately followed by a lcation change
		 */
	}

	/**
	 * 
	 */
	@Override
	public void onLocationChange(Point2D location) {

		/*
		 * quando il sistema e' inizializzato , il token di appartenenza non si
		 * conosce e bisgona chiederlo al MainTCC
		 */
		lastUpdateTimestamp = System.currentTimeMillis();

		if (!connected) {
			if (System.getProperty("testing") != null) {
				initializeForTesting(location);
				connected = true;
				return;
			}
			/* send the message to the Main TCC to ask for a belonging token */

			URL url;
			try {
				url = new URL(tccMainServerAddress);
				URLConnection httpURLConnection = url.openConnection();
				OutputStream os = httpURLConnection.getOutputStream();
				StringBuilder sb = new StringBuilder();
				sb.append("xPos=").append(location.getX()).append("&yPos=")
						.append(location.getY());
				os.write(sb.toString().getBytes());
				httpURLConnection.connect();

				InputStream is = httpURLConnection.getInputStream();
				BufferedReader br = new BufferedReader(
						new InputStreamReader(is));
				String line = null;
				sb = new StringBuilder();
				while ((line = br.readLine()) != null) {
					sb.append(line);
				}

				/*
				 * convert the stringBuilder into a jsonObject and read the
				 * result The result should contain the Token in which the
				 * connected vehicle is present; along with the token id , there
				 * is the TCC address in charge of the tokens' area
				 */

				// TODO read the data returned by the server ( it should use the
				// TCP protocol in order to guarantee the registration at first
				// time
				Token t = null;
				String tccAddress = null;
				int tccPort = 0;
				VehicleZone vz = new VehicleZone();
				vz.setTccAddress(InetSocketAddress.createUnresolved(tccAddress,
						tccPort));
				vz.setZoneToken(t);
				vn.getNeighbors().add(vz);

				/*
				 * after having validated the response , set the flag to
				 * connected; if the validation does not succeed, don't set the
				 * flag to connected so that the next time the authentication
				 * and registration is retried
				 */
				connected = true;

			} catch (MalformedURLException e) {
				e.printStackTrace();
			} catch (IOException e) {
				e.printStackTrace();
			}

		}

		if (connected) {
			try {

				/*
				 * send an update message to all the vehicles iy knows about and
				 * to all the zones actually present inside the neighborhood
				 */
				VehicleToVehicleMessage vm = new VehicleToVehicleMessage(
						Constants.VEHICLE_UPDATE_MESSAGE_CODE,
						sourceVehicle.getVehicleIdentificationNumber());
				vm.setBelongingToken(vn.getCurrentZone().getZoneToken());
				vm.setSpeed(sourceVehicle.getSpeed());
				vm.setUtmXPos(location.getX());
				vm.setUtmYPos(location.getY());
				vm.setDirection(sourceVehicle.getDirection());
				vm.setCurrentLaneId(sourceVehicle.getCurrentLaneId());
				vm.setTimestamp(System.currentTimeMillis());
				vncm.sendMessage(vm);

				checkTrafficLightsLogic();

			} catch (IOException e) {
				e.printStackTrace();
			}
		}
	}

	/**
	 * Main logic for traffic light speed reduction.</br> It suggests to reduce
	 * the speed if a traffic light is going to become green after a time
	 * <b>t</b> and the actual speed of the vehicle will bring him in front of
	 * the traffic light on t1<t , <br/>
	 * thus having to stop and restart when the traffic lights becomes green and
	 * consuming more fuel than the suggested case.
	 */
	private void checkTrafficLightsLogic() {
		/*
		 * read the traffic light's position, calculate the distance to reach
		 * the traffic light. if the current phase is RED , and given: -the
		 * instant when it will be GREEN -the current vehicle speed -> adjust
		 * the speed of the vehicle so that it can reach the traffic light when
		 * it becomes green, it can pass through and start accelerating again
		 */
		if (this.tccTrafficLightPhaseChange == null)
			return;

		if (isTrafficLightBehind(tccTrafficLightPhaseChange)) {
			sourceVehicle.setSpeed(-1);
			this.tccTrafficLightPhaseChange = null;
			return;
		}
		/*
		 * given the TL lastChangeTimestamp and when it will switch again ,
		 * calculate in milliseconds the absolute time when the switch will
		 * happen if(the nextPhase is RED-> alert to slow down the speed) else
		 * if( current phase is red and next phase is green , slow down in order
		 * to reach the traffic light at the time it will change
		 */

		double distanceFromTrafficLight = tccTrafficLightPhaseChange
				.getPosition().distance(sourceVehicle.getPosition());// meters
		long simTime = sourceVehicle.getCurrentTime();// milliseconds
		if (simTime == -1)
			return;

		int timeToSwitchInSeconds = (int) (tccTrafficLightPhaseChange
				.getAssumedTimeToExchange() - simTime) / 1000;
		float suggestedSpeed = (float) Math.floor(distanceFromTrafficLight
				/ timeToSwitchInSeconds);

		int alpha = 10;// seconds
		int timeToReachTheTrafficLightAtCurrentSpeed = (int) (distanceFromTrafficLight / sourceVehicle
				.getSpeed());// in seconds

		try {
			TRAFFICLIGHT_STATE currentState = TCCTrafficLightPhaseChange.TRAFFICLIGHT_STATE
					.fromState(tccTrafficLightPhaseChange.getCurrentState());
			TRAFFICLIGHT_STATE nextState = TCCTrafficLightPhaseChange.TRAFFICLIGHT_STATE
					.fromState(tccTrafficLightPhaseChange.getNextState());

			switch (currentState) {
				case GREEN :
					switch (nextState) {
						case RED :
						case YELLOW :
							/*
							 * if the vehicle proceeding at the current speed (
							 * higher than the suggested speed ) will reach the
							 * traffic light after the it changed to RED/YELLOW,
							 * it's better to change the current speed to the
							 * suggested speed, otherwise leave the speed as it
							 * is
							 */
							if (timeToReachTheTrafficLightAtCurrentSpeed >= timeToSwitchInSeconds
									+ alpha
									&& suggestedSpeed < sourceVehicle
											.getSpeed()) {
								/*
								 * System.out.println("vehicle "+sourceVehicle.
								 * getVehicleIdentificationNumber
								 * ()+" at speed "+sourceVehicle.getSpeed()+"" +
								 * ": if you slow down to "+suggestedSpeed +
								 * " you can reduce consumption becuase the Traffic light is going to become yellow/red"
								 * );
								 */
								sourceVehicle.setSpeed(suggestedSpeed);
							}
							break;
						default :
							// nothing to do
							break;
					}
					break;
				case RED :
					switch (nextState) {
						case GREEN :
							if (timeToReachTheTrafficLightAtCurrentSpeed < timeToSwitchInSeconds
									+ alpha
									&& suggestedSpeed < sourceVehicle
											.getSpeed()) {
								/*
								 * System.out.println("vehicle : "+sourceVehicle.
								 * getVehicleIdentificationNumber
								 * ()+", at speed "+sourceVehicle.getSpeed()+" "
								 * +
								 * "if you reuce the speed at "+suggestedSpeed+
								 * " you can reduce the fuel consumption because you don't need to stop"
								 * );
								 */
								sourceVehicle.setSpeed(suggestedSpeed);
							}

							break;
						case YELLOW :
							break;
						default :
							// nothing to do
							break;
					}
					break;
				case YELLOW :
					switch (nextState) {
						case RED :
							int securityDistanceThreshold = 30;// in meters
							int securitySpeedThreshold = 30;// in m/s
							int minimumReactionTime = 30;// in seconds
							if (distanceFromTrafficLight < securityDistanceThreshold
									&& sourceVehicle.getSpeed() > securitySpeedThreshold
									&& timeToReachTheTrafficLightAtCurrentSpeed > timeToSwitchInSeconds
									&& Math.abs(timeToReachTheTrafficLightAtCurrentSpeed
											- timeToSwitchInSeconds) < minimumReactionTime) {
								sourceVehicle.setSpeed(suggestedSpeed);
								System.err
										.println("!! WARNING !! the traffic light is going to become RED and your speed is to high");
							} else if (timeToReachTheTrafficLightAtCurrentSpeed > timeToSwitchInSeconds) {
								sourceVehicle.setSpeed(suggestedSpeed);
							}
							break;
						case GREEN :
							break;
						default :
							// nothing to do
							break;
					}
					break;
			}
		} catch (Exception e) {
			e.printStackTrace();
		}
	}

	private void initializeForTesting(Point2D location) {
		// TODO Auto-generated method stub

		TestZones ts = TestZones.getInstance();
		VehicleZone vehicleZone = ts.getVehicleStartZoneToken(location.getX(),
				location.getY());
		System.out.println("initializing vehicle "
				+ sourceVehicle.getVehicleIdentificationNumber()
				+ " at position " + location.getX() + "," + location.getY());
		if (vehicleZone != null) {
			System.out.println("vehicle "
					+ sourceVehicle.getVehicleIdentificationNumber()
					+ " starts with token : "
					+ vehicleZone.getZoneToken().toString());
			vn.getNeighbors().add(vehicleZone);
			vn.setCurrentZone(vehicleZone);
		}
		connected = true;
	}

	/**
	 * The vehicle is now connected to the system
	 */
	public void onConnect() {
		System.out
				.println("############## vehicle connected to the system #############");
	}

	/**
	 * The vehicle is now disconnected from the system
	 */
	public void onDisconnect() {
		/* send GoodBye Message to all the currently active zones */
		connected = false;
		for (VehicleZone vz : vn.getNeighbors()) {
			try {

				GoodbyeMessage gb = new GoodbyeMessage(vz.getZoneToken());

				gb.setForTCC(true);
				gb.setForZones(false);
				gb.setTccAddress(vz.getTccAddress());
				gb.setVehicleIdentifier(sourceVehicle
						.getVehicleIdentificationNumber());
				vncm.sendMessage(gb);

			} catch (IOException e) {
				e.printStackTrace();
			}
		}
		System.out
				.println("############## vehicle disconnected from the system #############");
	}

	@Override
	public void onLaneChange(String laneId) {
	}

}
